

<!DOCTYPE html>
<!--[if IE 8]><html class="no-js lt-ie9" lang="en" > <![endif]-->
<!--[if gt IE 8]><!--> <html class="no-js" lang="en" > <!--<![endif]-->
<head>
  <meta charset="utf-8">
  
  <meta name="viewport" content="width=device-width, initial-scale=1.0">
  
  <title>ukfm.model.localization &mdash; Unscented Kalman Filtering on (Parallelizable) Manifolds alpha documentation</title>
  

  
  

  

  
  
    

  

  
  
    <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
  

  
    <link rel="stylesheet" href="../../../_static/gallery.css" type="text/css" />
  
    <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
  

  
        <link rel="index" title="Index"
              href="../../../genindex.html"/>
        <link rel="search" title="Search" href="../../../search.html"/>
    <link rel="top" title="Unscented Kalman Filtering on (Parallelizable) Manifolds alpha documentation" href="../../../index.html"/>
        <link rel="up" title="Module code" href="../../index.html"/> 

  
  <script src="../../../_static/js/modernizr.min.js"></script>

</head>

<body class="wy-body-for-nav" role="document">

  <div class="wy-grid-for-nav">

    
    <nav data-toggle="wy-nav-shift" class="wy-nav-side">
      <div class="wy-side-scroll">
        <div class="wy-side-nav-search">
          

          
            <a href="../../../index.html" class="icon icon-home"> Unscented Kalman Filtering on (Parallelizable) Manifolds
          

          
            
            <img src="../../../_static/blacklogo.png" class="logo" />
          
          </a>

          
            
            
              <div class="version">
                1.0
              </div>
            
          

          
<div role="search">
  <form id="rtd-search-form" class="wy-form" action="../../../search.html" method="get">
    <input type="text" name="q" placeholder="Search docs" />
    <input type="hidden" name="check_keywords" value="yes" />
    <input type="hidden" name="area" value="default" />
  </form>
</div>

          
        </div>

        <div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="main navigation">
          
            
            
                <ul>
<li class="toctree-l1"><a class="reference internal" href="../../../install.html">Installation</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../auto_examples/localization.html">Tutorial</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../examples.html">Examples</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../benchmarks.html">Benchmarks</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../filter.html">Filters</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../model.html">Models</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../geometry.html">Lie Groups</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../matlab.html">Matlab</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../license.html">License</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../bibliography.html">Bibliography</a></li>
</ul>

            
          
        </div>
      </div>
    </nav>

    <section data-toggle="wy-nav-shift" class="wy-nav-content-wrap">

      
      <nav class="wy-nav-top" role="navigation" aria-label="top navigation">
        <i data-toggle="wy-nav-top" class="fa fa-bars"></i>
        <a href="../../../index.html">Unscented Kalman Filtering on (Parallelizable) Manifolds</a>
      </nav>


      
      <div class="wy-nav-content">
        <div class="rst-content">
          





<div role="navigation" aria-label="breadcrumbs navigation">
  <ul class="wy-breadcrumbs">
    <li><a href="../../../index.html">Docs</a> &raquo;</li>
      
          <li><a href="../../index.html">Module code</a> &raquo;</li>
      
    <li>ukfm.model.localization</li>
      <li class="wy-breadcrumbs-aside">
        
          
        
      </li>
  </ul>
  <hr/>
</div>
          <div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
           <div itemprop="articleBody">
            
  <h1>Source code for ukfm.model.localization</h1><div class="highlight"><pre>
<span></span><span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>
<span class="kn">from</span> <span class="nn">ukfm.geometry.so2</span> <span class="k">import</span> <span class="n">SO2</span>
<span class="kn">from</span> <span class="nn">ukfm.geometry.se2</span> <span class="k">import</span> <span class="n">SE2</span>
<span class="kn">import</span> <span class="nn">matplotlib.pyplot</span> <span class="k">as</span> <span class="nn">plt</span>
<span class="kn">import</span> <span class="nn">os</span>


<div class="viewcode-block" id="LOCALIZATION"><a class="viewcode-back" href="../../../model.html#ukfm.LOCALIZATION">[docs]</a><span class="k">class</span> <span class="nc">LOCALIZATION</span><span class="p">:</span>
    <span class="sd">&quot;&quot;&quot;2D Robot localization based on odometry and GNSS (robot position) </span>
<span class="sd">    measurements. See a text description in :cite:`barrauInvariant2017`, Section</span>
<span class="sd">    IV.</span>

<span class="sd">    :var T: sequence time (s).</span>
<span class="sd">    :var odo_freq: odometry frequency (Hz).</span>
<span class="sd">    &quot;&quot;&quot;</span>

    <span class="n">J</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([[</span><span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="mi">1</span><span class="p">],</span>
                  <span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">0</span><span class="p">]])</span>
    <span class="n">data_dir</span> <span class="o">=</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">dirname</span><span class="p">(</span><span class="vm">__file__</span><span class="p">),</span> <span class="s2">&quot;../../examples/data/&quot;</span><span class="p">)</span>

<div class="viewcode-block" id="LOCALIZATION.STATE"><a class="viewcode-back" href="../../../model.html#ukfm.LOCALIZATION.STATE">[docs]</a>    <span class="k">class</span> <span class="nc">STATE</span><span class="p">:</span>
        <span class="sd">&quot;&quot;&quot;State of the system.</span>

<span class="sd">        It represents the orientation and the position of the robot.</span>

<span class="sd">        .. math::</span>

<span class="sd">            \\boldsymbol{\\chi} \in \\mathcal{M} = \\left\\{ \\begin{matrix} </span>
<span class="sd">           \\mathbf{C} \in SO(2),</span>
<span class="sd">            \\mathbf{p} \in \\mathbb R^2</span>
<span class="sd">           \\end{matrix} \\right\\}</span>

<span class="sd">        :ivar Rot: rotation matrix :math:`\mathbf{C}`.</span>
<span class="sd">        :ivar p: position vector :math:`\mathbf{p}`.</span>
<span class="sd">        &quot;&quot;&quot;</span>

        <span class="k">def</span> <span class="nf">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">Rot</span><span class="p">,</span> <span class="n">p</span><span class="p">):</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">Rot</span> <span class="o">=</span> <span class="n">Rot</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">p</span> <span class="o">=</span> <span class="n">p</span></div>

<div class="viewcode-block" id="LOCALIZATION.INPUT"><a class="viewcode-back" href="../../../model.html#ukfm.LOCALIZATION.INPUT">[docs]</a>    <span class="k">class</span> <span class="nc">INPUT</span><span class="p">:</span>
        <span class="sd">&quot;&quot;&quot;Input of the propagation model.</span>

<span class="sd">        The input are the robot velocities that can be obtained from a</span>
<span class="sd">        differential wheel system.</span>

<span class="sd">        .. math::</span>

<span class="sd">            \\boldsymbol{\\omega} \in \\mathcal{U} = \\left\\{ \\begin{matrix}</span>
<span class="sd">            \\mathbf{v} \in \\mathbb R^2,</span>
<span class="sd">            \\omega \in \\mathbb R </span>
<span class="sd">            \\end{matrix} \\right\\}</span>

<span class="sd">        :ivar v: robot forward and lateral velocities :math:`\\mathbf{v}`.</span>
<span class="sd">        :ivar gyro: robot orientation velocity :math:`\\omega`.</span>
<span class="sd">        &quot;&quot;&quot;</span>

        <span class="k">def</span> <span class="nf">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">v</span><span class="p">,</span> <span class="n">gyro</span><span class="p">):</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">v</span> <span class="o">=</span> <span class="n">v</span>
            <span class="bp">self</span><span class="o">.</span><span class="n">gyro</span> <span class="o">=</span> <span class="n">gyro</span></div>

    <span class="k">def</span> <span class="nf">__init__</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">T</span><span class="p">,</span> <span class="n">odo_freq</span><span class="p">):</span>
        <span class="c1"># sequence time (s)</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">T</span> <span class="o">=</span> <span class="mi">40</span>
        <span class="c1"># odometry frequency (Hz)</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">odo_freq</span> <span class="o">=</span> <span class="mi">100</span>
        <span class="c1"># total number of timestamps</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">N</span> <span class="o">=</span> <span class="n">T</span><span class="o">*</span><span class="n">odo_freq</span>
        <span class="c1"># integration step (s)</span>
        <span class="bp">self</span><span class="o">.</span><span class="n">dt</span> <span class="o">=</span> <span class="mi">1</span><span class="o">/</span><span class="n">odo_freq</span>

<div class="viewcode-block" id="LOCALIZATION.f"><a class="viewcode-back" href="../../../model.html#ukfm.LOCALIZATION.f">[docs]</a>    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">f</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">,</span> <span class="n">omega</span><span class="p">,</span> <span class="n">w</span><span class="p">,</span> <span class="n">dt</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot; Propagation function.</span>

<span class="sd">        .. math::</span>

<span class="sd">            \\mathbf{C}_{n+1}  &amp;= \\mathbf{C}_{n} \\exp\\left(\\left(\\omega + </span>
<span class="sd">            \\mathbf{w}^{(2)} \\right) dt\\right)  \\\\</span>
<span class="sd">            \\mathbf{p}_{n+1}  &amp;= \\mathbf{p}_{n} + \\left( \\mathbf{v}_{n} + </span>
<span class="sd">            \\mathbf{w}^{(0:2)} \\right) dt</span>

<span class="sd">        :var state: state :math:`\\boldsymbol{\\chi}`.</span>
<span class="sd">        :var omega: input :math:`\\boldsymbol{\\omega}`.</span>
<span class="sd">        :var w: noise :math:`\\mathbf{w}`.</span>
<span class="sd">        :var dt: integration step :math:`dt` (s).</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">new_state</span> <span class="o">=</span> <span class="bp">cls</span><span class="o">.</span><span class="n">STATE</span><span class="p">(</span>
            <span class="n">Rot</span><span class="o">=</span><span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">SO2</span><span class="o">.</span><span class="n">exp</span><span class="p">((</span><span class="n">omega</span><span class="o">.</span><span class="n">gyro</span> <span class="o">+</span> <span class="n">w</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span><span class="o">*</span><span class="n">dt</span><span class="p">)),</span>
            <span class="n">p</span><span class="o">=</span><span class="n">state</span><span class="o">.</span><span class="n">p</span> <span class="o">+</span> <span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">omega</span><span class="o">.</span><span class="n">v</span> <span class="o">+</span> <span class="n">w</span><span class="p">[:</span><span class="mi">2</span><span class="p">])</span><span class="o">*</span><span class="n">dt</span>
        <span class="p">)</span>
        <span class="k">return</span> <span class="n">new_state</span></div>

<div class="viewcode-block" id="LOCALIZATION.h"><a class="viewcode-back" href="../../../model.html#ukfm.LOCALIZATION.h">[docs]</a>    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">h</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot; Observation function.</span>

<span class="sd">        .. math::</span>

<span class="sd">            h\\left(\\boldsymbol{\\chi}\\right)  = \\mathbf{p}</span>

<span class="sd">        :var state: state :math:`\\boldsymbol{\\chi}`.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">y</span> <span class="o">=</span> <span class="n">state</span><span class="o">.</span><span class="n">p</span>
        <span class="k">return</span> <span class="n">y</span></div>

<div class="viewcode-block" id="LOCALIZATION.phi"><a class="viewcode-back" href="../../../model.html#ukfm.LOCALIZATION.phi">[docs]</a>    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">phi</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">,</span> <span class="n">xi</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Retraction.</span>

<span class="sd">        .. math::</span>

<span class="sd">          \\varphi\\left(\\boldsymbol{\\chi}, \\boldsymbol{\\xi}\\right) = </span>
<span class="sd">          \\left( \\begin{matrix}</span>
<span class="sd">            \\mathbf{C} \\exp\\left(\\boldsymbol{\\xi}^{(0)}\\right) \\\\</span>
<span class="sd">            \\mathbf{p} + \\boldsymbol{\\xi}^{(1:3)}</span>
<span class="sd">           \\end{matrix} \\right)</span>

<span class="sd">        The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SO(2) </span>
<span class="sd">        \\times \\mathbb R^2`.</span>

<span class="sd">        Its corresponding inverse operation is</span>
<span class="sd">        :meth:`~ukfm.LOCALIZATION.phi_inv`.</span>

<span class="sd">        :var state: state :math:`\\boldsymbol{\\chi}`.</span>
<span class="sd">        :var xi: state uncertainty :math:`\\boldsymbol{\\xi}`.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">new_state</span> <span class="o">=</span> <span class="bp">cls</span><span class="o">.</span><span class="n">STATE</span><span class="p">(</span>
            <span class="n">Rot</span><span class="o">=</span><span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">SO2</span><span class="o">.</span><span class="n">exp</span><span class="p">(</span><span class="n">xi</span><span class="p">[</span><span class="mi">0</span><span class="p">])),</span>
            <span class="n">p</span><span class="o">=</span><span class="n">state</span><span class="o">.</span><span class="n">p</span> <span class="o">+</span> <span class="n">xi</span><span class="p">[</span><span class="mi">1</span><span class="p">:]</span>
        <span class="p">)</span>
        <span class="k">return</span> <span class="n">new_state</span></div>

<div class="viewcode-block" id="LOCALIZATION.phi_inv"><a class="viewcode-back" href="../../../model.html#ukfm.LOCALIZATION.phi_inv">[docs]</a>    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">phi_inv</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">,</span> <span class="n">hat_state</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Inverse retraction.</span>

<span class="sd">        .. math::</span>

<span class="sd">          \\varphi^{-1}_{\\boldsymbol{\\hat{\\chi}}}\\left(\\boldsymbol{\\chi}</span>
<span class="sd">          \\right) = \\left( \\begin{matrix}</span>
<span class="sd">            \\log\\left(\\mathbf{C} \\mathbf{\\hat{C}}^T\\right) \\\\</span>
<span class="sd">            \\mathbf{p} - \\mathbf{\\hat{p}} </span>
<span class="sd">           \\end{matrix} \\right)</span>

<span class="sd">        The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SO(2)</span>
<span class="sd">        \\times \\mathbb R^2`.</span>

<span class="sd">        Its corresponding retraction is :meth:`~ukfm.LOCALIZATION.phi`.</span>

<span class="sd">        :var state: state :math:`\\boldsymbol{\\chi}`.</span>
<span class="sd">        :var hat_state: noise-free state :math:`\\boldsymbol{\hat{\\chi}}`.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">xi</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">hstack</span><span class="p">([</span><span class="n">SO2</span><span class="o">.</span><span class="n">log</span><span class="p">(</span><span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">hat_state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">T</span><span class="p">)),</span>
                        <span class="n">state</span><span class="o">.</span><span class="n">p</span> <span class="o">-</span> <span class="n">hat_state</span><span class="o">.</span><span class="n">p</span><span class="p">])</span>
        <span class="k">return</span> <span class="n">xi</span></div>

<div class="viewcode-block" id="LOCALIZATION.left_phi"><a class="viewcode-back" href="../../../model.html#ukfm.LOCALIZATION.left_phi">[docs]</a>    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">left_phi</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">,</span> <span class="n">xi</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Retraction.</span>

<span class="sd">        .. math::</span>

<span class="sd">          \\varphi\\left(\\boldsymbol{\\chi}, \\boldsymbol{\\xi}\\right) = </span>
<span class="sd">          \\left( \\begin{matrix}</span>
<span class="sd">               \\mathbf{C} \\mathbf{C}_\\mathbf{T} \\\\</span>
<span class="sd">                \\mathbf{p} + \\mathbf{C} \\mathbf{r}</span>
<span class="sd">          \\end{matrix} \\right)</span>

<span class="sd">        where</span>

<span class="sd">        .. math::</span>
<span class="sd">                \\mathbf{T} = \\exp\\left(\\boldsymbol{\\xi}\\right) = </span>
<span class="sd">                \\begin{bmatrix}</span>
<span class="sd">                    \\mathbf{C}_\\mathbf{T} &amp; \\mathbf{r} \\\\</span>
<span class="sd">                    \\mathbf{0}^T &amp; 1</span>
<span class="sd">                \\end{bmatrix}</span>

<span class="sd">        The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SE(2)`</span>
<span class="sd">        with left multiplication.</span>

<span class="sd">        Its corresponding inverse operation is</span>
<span class="sd">        :meth:`~ukfm.LOCALIZATION.left_phi_inv`.</span>

<span class="sd">        :var state: state :math:`\\boldsymbol{\\chi}`.</span>
<span class="sd">        :var xi: state uncertainty :math:`\\boldsymbol{\\xi}`.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">chi</span> <span class="o">=</span> <span class="n">SE2</span><span class="o">.</span><span class="n">exp</span><span class="p">(</span><span class="n">xi</span><span class="p">)</span>
        <span class="n">new_state</span> <span class="o">=</span> <span class="bp">cls</span><span class="o">.</span><span class="n">STATE</span><span class="p">(</span>
            <span class="n">Rot</span><span class="o">=</span><span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">chi</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]),</span>
            <span class="n">p</span><span class="o">=</span><span class="n">state</span><span class="o">.</span><span class="n">p</span> <span class="o">+</span> <span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">chi</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">])</span>
        <span class="p">)</span>
        <span class="k">return</span> <span class="n">new_state</span></div>

<div class="viewcode-block" id="LOCALIZATION.left_phi_inv"><a class="viewcode-back" href="../../../model.html#ukfm.LOCALIZATION.left_phi_inv">[docs]</a>    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">left_phi_inv</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">,</span> <span class="n">hat_state</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Inverse retraction.</span>

<span class="sd">        .. math::</span>

<span class="sd">          \\varphi^{-1}_{\\boldsymbol{\\hat{\\chi}}}\\left(\\boldsymbol{\\chi}</span>
<span class="sd">          \\right) = \\log\\left(</span>
<span class="sd">              \\boldsymbol{\chi}^{-1} \\boldsymbol{\\hat{\\chi}} \\right)</span>

<span class="sd">        The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SE(2)` </span>
<span class="sd">        with left multiplication.</span>

<span class="sd">        Its corresponding retraction is :meth:`~ukfm.LOCALIZATION.left_phi`.</span>

<span class="sd">        :arg state: state :math:`\\boldsymbol{\\chi}`.</span>
<span class="sd">        :arg hat_state: noise-free state :math:`\\boldsymbol{\hat{\\chi}}`.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">chi</span> <span class="o">=</span> <span class="bp">cls</span><span class="o">.</span><span class="n">state2chi</span><span class="p">(</span><span class="n">state</span><span class="p">)</span>
        <span class="n">hat_chi</span> <span class="o">=</span> <span class="bp">cls</span><span class="o">.</span><span class="n">state2chi</span><span class="p">(</span><span class="n">hat_state</span><span class="p">)</span>
        <span class="n">xi</span> <span class="o">=</span> <span class="n">SE2</span><span class="o">.</span><span class="n">log</span><span class="p">(</span><span class="n">SE2</span><span class="o">.</span><span class="n">inv</span><span class="p">(</span><span class="n">chi</span><span class="p">)</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">hat_chi</span><span class="p">))</span>
        <span class="k">return</span> <span class="n">xi</span></div>

<div class="viewcode-block" id="LOCALIZATION.right_phi"><a class="viewcode-back" href="../../../model.html#ukfm.LOCALIZATION.right_phi">[docs]</a>    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">right_phi</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">,</span> <span class="n">xi</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Retraction.</span>

<span class="sd">        .. math::</span>

<span class="sd">          \\varphi\\left(\\boldsymbol{\\chi}, \\boldsymbol{\\xi}\\right) = </span>
<span class="sd">          \\left( \\begin{matrix}</span>
<span class="sd">                \\mathbf{C}_\\mathbf{T} \\mathbf{C}  \\\\</span>
<span class="sd">                 \\mathbf{C}_\\mathbf{T}\\mathbf{p} +  \\mathbf{r}</span>
<span class="sd">             \\end{matrix} \\right)</span>

<span class="sd">        where</span>

<span class="sd">        .. math::</span>
<span class="sd">        </span>
<span class="sd">                \\mathbf{T} = \\exp\\left(\\boldsymbol{\\xi}\\right) </span>
<span class="sd">                = \\begin{bmatrix}</span>
<span class="sd">                    \\mathbf{C}_\\mathbf{T} &amp;\\mathbf{r} \\\\</span>
<span class="sd">                    \\mathbf{0}^T &amp; 1</span>
<span class="sd">                \\end{bmatrix}</span>

<span class="sd">        The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SE(2)` </span>
<span class="sd">        with right multiplication.</span>

<span class="sd">        Its corresponding inverse operation is </span>
<span class="sd">        :meth:`~ukfm.LOCALIZATION.right_phi_inv`.</span>

<span class="sd">        :var state: state :math:`\\boldsymbol{\\chi}`.</span>
<span class="sd">        :var xi: state uncertainty :math:`\\boldsymbol{\\xi}`.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">chi</span> <span class="o">=</span> <span class="n">SE2</span><span class="o">.</span><span class="n">exp</span><span class="p">(</span><span class="n">xi</span><span class="p">)</span>
        <span class="n">new_state</span> <span class="o">=</span> <span class="bp">cls</span><span class="o">.</span><span class="n">STATE</span><span class="p">(</span>
            <span class="n">Rot</span><span class="o">=</span><span class="n">chi</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="p">),</span>
            <span class="n">p</span><span class="o">=</span><span class="n">chi</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">+</span> <span class="n">chi</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">state</span><span class="o">.</span><span class="n">p</span><span class="p">)</span>
        <span class="p">)</span>
        <span class="k">return</span> <span class="n">new_state</span></div>

<div class="viewcode-block" id="LOCALIZATION.right_phi_inv"><a class="viewcode-back" href="../../../model.html#ukfm.LOCALIZATION.right_phi_inv">[docs]</a>    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">right_phi_inv</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">,</span> <span class="n">hat_state</span><span class="p">):</span>
        <span class="sd">&quot;&quot;&quot;Inverse retraction.</span>

<span class="sd">        .. math::</span>

<span class="sd">          \\varphi^{-1}_{\\boldsymbol{\\hat{\\chi}}}\\left(\\boldsymbol{\\chi}</span>
<span class="sd">          \\right) = \\log\\left(</span>
<span class="sd">              \\boldsymbol{\\hat{\\chi}}^{-1} \\boldsymbol{\\chi} \\right)</span>

<span class="sd">        The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SE(2)` </span>
<span class="sd">        with right multiplication.</span>

<span class="sd">        Its corresponding retraction is :meth:`~ukfm.LOCALIZATION.right_phi`.</span>

<span class="sd">        :var state: state :math:`\\boldsymbol{\\chi}`.</span>
<span class="sd">        :var hat_state: noise-free state :math:`\\boldsymbol{\hat{\\chi}}`.</span>
<span class="sd">        &quot;&quot;&quot;</span>
        <span class="n">chi</span> <span class="o">=</span> <span class="bp">cls</span><span class="o">.</span><span class="n">state2chi</span><span class="p">(</span><span class="n">state</span><span class="p">)</span>
        <span class="n">hat_chi</span> <span class="o">=</span> <span class="bp">cls</span><span class="o">.</span><span class="n">state2chi</span><span class="p">(</span><span class="n">hat_state</span><span class="p">)</span>
        <span class="n">xi</span> <span class="o">=</span> <span class="n">SE2</span><span class="o">.</span><span class="n">log</span><span class="p">(</span><span class="n">hat_chi</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">SE2</span><span class="o">.</span><span class="n">inv</span><span class="p">(</span><span class="n">chi</span><span class="p">)))</span>
        <span class="k">return</span> <span class="n">xi</span></div>

    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">ekf_FG_ana</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">,</span> <span class="n">omega</span><span class="p">,</span> <span class="n">dt</span><span class="p">):</span>
        <span class="n">F</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
        <span class="n">F</span><span class="p">[</span><span class="mi">1</span><span class="p">:,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="bp">cls</span><span class="o">.</span><span class="n">J</span><span class="p">)</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">omega</span><span class="o">.</span><span class="n">v</span><span class="p">)</span><span class="o">*</span><span class="n">dt</span>
        <span class="n">G</span> <span class="o">=</span> <span class="n">dt</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
        <span class="n">G</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">dt</span><span class="o">*</span><span class="n">state</span><span class="o">.</span><span class="n">Rot</span>
        <span class="k">return</span> <span class="n">F</span><span class="p">,</span> <span class="n">G</span>

    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">ekf_H_ana</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">):</span>
        <span class="n">H</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">hstack</span><span class="p">([</span><span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">)),</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">2</span><span class="p">)])</span>
        <span class="k">return</span> <span class="n">H</span>

    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">iekf_FG_ana</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">,</span> <span class="n">omega</span><span class="p">,</span> <span class="n">dt</span><span class="p">):</span>
        <span class="n">F</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
        <span class="n">F</span><span class="p">[</span><span class="mi">1</span><span class="p">:,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="bp">cls</span><span class="o">.</span><span class="n">J</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">omega</span><span class="o">.</span><span class="n">v</span><span class="p">)</span><span class="o">*</span><span class="n">dt</span>
        <span class="n">F</span><span class="p">[</span><span class="mi">1</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">omega</span><span class="o">.</span><span class="n">gyro</span><span class="o">*</span><span class="n">dt</span>
        <span class="n">F</span><span class="p">[</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="o">-</span><span class="n">omega</span><span class="o">.</span><span class="n">gyro</span><span class="o">*</span><span class="n">dt</span>
        <span class="n">G</span> <span class="o">=</span> <span class="n">dt</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">F</span><span class="p">,</span> <span class="n">G</span>

    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">iekf_H_ana</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">):</span>
        <span class="n">H</span> <span class="o">=</span> <span class="n">state</span><span class="o">.</span><span class="n">Rot</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">hstack</span><span class="p">([</span><span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="mi">2</span><span class="p">,</span> <span class="mi">1</span><span class="p">)),</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">2</span><span class="p">)]))</span>
        <span class="k">return</span> <span class="n">H</span>

    <span class="k">def</span> <span class="nf">simu_f</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">odo_std</span><span class="p">,</span> <span class="n">radius</span><span class="p">):</span>
        <span class="c1"># set input</span>
        <span class="n">v</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="mi">2</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">radius</span><span class="o">/</span><span class="bp">self</span><span class="o">.</span><span class="n">T</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>  <span class="c1"># forward speed (m/s)</span>
        <span class="n">gyro</span> <span class="o">=</span> <span class="mi">2</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">/</span><span class="bp">self</span><span class="o">.</span><span class="n">T</span>  <span class="c1"># angular speed (rad/s)</span>

        <span class="c1"># set noise to zero to compute the true trajectory</span>
        <span class="n">w</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>

        <span class="c1"># init variables at zero and do for loop</span>
        <span class="n">omegas</span> <span class="o">=</span> <span class="p">[]</span>
        <span class="n">states</span> <span class="o">=</span> <span class="p">[</span><span class="bp">self</span><span class="o">.</span><span class="n">STATE</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">2</span><span class="p">),</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="mi">2</span><span class="p">))]</span>

        <span class="k">for</span> <span class="n">n</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="mi">1</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">):</span>
            <span class="c1"># true input</span>
            <span class="n">omegas</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">INPUT</span><span class="p">(</span><span class="n">v</span><span class="p">,</span> <span class="n">gyro</span><span class="p">))</span>
            <span class="c1"># propagate state</span>
            <span class="n">states</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">f</span><span class="p">(</span><span class="n">states</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="n">w</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">dt</span><span class="p">))</span>
            <span class="c1"># noisy input</span>
            <span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">]</span><span class="o">.</span><span class="n">v</span> <span class="o">=</span> <span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">]</span><span class="o">.</span><span class="n">v</span> <span class="o">+</span> <span class="n">odo_std</span><span class="p">[:</span><span class="mi">2</span><span class="p">]</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">randn</span><span class="p">(</span><span class="mi">2</span><span class="p">)</span>
            <span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">]</span><span class="o">.</span><span class="n">gyro</span> <span class="o">=</span> <span class="n">omegas</span><span class="p">[</span><span class="n">n</span><span class="o">-</span><span class="mi">1</span><span class="p">]</span><span class="o">.</span><span class="n">gyro</span> <span class="o">+</span> <span class="n">odo_std</span><span class="p">[</span><span class="mi">2</span><span class="p">]</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">randn</span><span class="p">(</span><span class="mi">1</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">states</span><span class="p">,</span> <span class="n">omegas</span>

    <span class="k">def</span> <span class="nf">simu_h</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">states</span><span class="p">,</span> <span class="n">gps_freq</span><span class="p">,</span> <span class="n">gps_std</span><span class="p">):</span>
        <span class="c1"># vector to identify when measurements happen</span>
        <span class="n">one_hot_ys</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
        <span class="n">one_hot_ys</span><span class="p">[::</span><span class="nb">int</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">odo_freq</span><span class="o">/</span><span class="n">gps_freq</span><span class="p">)]</span> <span class="o">=</span> <span class="mi">1</span>
        <span class="c1"># odo_freq/gps_freq must be integer</span>
        <span class="n">idxs</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">where</span><span class="p">(</span><span class="n">one_hot_ys</span> <span class="o">==</span> <span class="mi">1</span><span class="p">)[</span><span class="mi">0</span><span class="p">]</span>  <span class="c1"># indexes where measurement happen</span>

        <span class="c1"># total number of measurements</span>
        <span class="n">K</span> <span class="o">=</span> <span class="n">idxs</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
        <span class="n">ys</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="n">K</span><span class="p">,</span> <span class="mi">2</span><span class="p">))</span>
        <span class="k">for</span> <span class="n">k</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">K</span><span class="p">):</span>
            <span class="n">ys</span><span class="p">[</span><span class="n">k</span><span class="p">]</span> <span class="o">=</span> <span class="n">states</span><span class="p">[</span><span class="n">idxs</span><span class="p">[</span><span class="n">k</span><span class="p">]]</span><span class="o">.</span><span class="n">p</span> <span class="o">+</span> <span class="n">gps_std</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">randn</span><span class="p">(</span><span class="mi">2</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">ys</span><span class="p">,</span> <span class="n">one_hot_ys</span>

    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">state2chi</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">state</span><span class="p">):</span>
        <span class="n">chi</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>
        <span class="n">chi</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="p">:</span><span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">state</span><span class="o">.</span><span class="n">Rot</span>
        <span class="n">chi</span><span class="p">[:</span><span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">=</span> <span class="n">state</span><span class="o">.</span><span class="n">p</span>
        <span class="k">return</span> <span class="n">chi</span>

    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">get_states</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">states</span><span class="p">,</span> <span class="n">N</span><span class="p">):</span>
        <span class="n">Rots</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="n">N</span><span class="p">,</span> <span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">))</span>
        <span class="n">ps</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="n">N</span><span class="p">,</span> <span class="mi">2</span><span class="p">))</span>
        <span class="k">for</span> <span class="n">n</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">N</span><span class="p">):</span>
            <span class="n">Rots</span><span class="p">[</span><span class="n">n</span><span class="p">]</span> <span class="o">=</span> <span class="n">states</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">.</span><span class="n">Rot</span>
            <span class="n">ps</span><span class="p">[</span><span class="n">n</span><span class="p">]</span> <span class="o">=</span> <span class="n">states</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">.</span><span class="n">p</span>
        <span class="k">return</span> <span class="n">Rots</span><span class="p">,</span> <span class="n">ps</span>

    <span class="k">def</span> <span class="nf">errors</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">Rots</span><span class="p">,</span> <span class="n">hat_Rots</span><span class="p">,</span> <span class="n">ps</span><span class="p">,</span> <span class="n">hat_ps</span><span class="p">):</span>
        <span class="n">errors</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">,</span> <span class="mi">3</span><span class="p">))</span>
        <span class="k">for</span> <span class="n">n</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">):</span>
            <span class="n">errors</span><span class="p">[</span><span class="n">n</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">SO2</span><span class="o">.</span><span class="n">log</span><span class="p">(</span><span class="n">Rots</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">.</span><span class="n">T</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">hat_Rots</span><span class="p">[</span><span class="n">n</span><span class="p">]))</span>
        <span class="n">errors</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">:</span><span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="n">ps</span><span class="o">-</span><span class="n">hat_ps</span>
        <span class="k">return</span> <span class="n">errors</span>

    <span class="k">def</span> <span class="nf">plot_results</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">ukf_states</span><span class="p">,</span> <span class="n">ukf_Ps</span><span class="p">,</span> <span class="n">states</span><span class="p">,</span> <span class="n">ys</span><span class="p">):</span>

        <span class="n">t</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">linspace</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">T</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>

        <span class="n">Rots</span><span class="p">,</span> <span class="n">ps</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">states</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
        <span class="n">ukf_Rots</span><span class="p">,</span> <span class="n">ukf_ps</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">ukf_states</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
        <span class="n">errors</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">errors</span><span class="p">(</span><span class="n">Rots</span><span class="p">,</span> <span class="n">ukf_Rots</span><span class="p">,</span> <span class="n">ps</span><span class="p">,</span> <span class="n">ukf_ps</span><span class="p">)</span>
        <span class="c1"># add position</span>
        <span class="n">errors</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">errors</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span><span class="o">**</span><span class="mi">2</span> <span class="o">+</span> <span class="n">errors</span><span class="p">[:,</span> <span class="mi">2</span><span class="p">]</span><span class="o">**</span><span class="mi">2</span><span class="p">)</span>

        <span class="n">fig</span><span class="p">,</span> <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">subplots</span><span class="p">(</span><span class="n">figsize</span><span class="o">=</span><span class="p">(</span><span class="mi">10</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="n">xlabel</span><span class="o">=</span><span class="s1">&#39;$x$ (m)&#39;</span><span class="p">,</span> <span class="n">ylabel</span><span class="o">=</span><span class="s1">&#39;$y$ (m)&#39;</span><span class="p">,</span> <span class="n">title</span><span class="o">=</span><span class="s2">&quot;Robot position&quot;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">linewidth</span><span class="o">=</span><span class="mi">2</span><span class="p">,</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;black&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">scatter</span><span class="p">(</span><span class="n">ys</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">ys</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;red&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">ukf_ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">ukf_ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">legend</span><span class="p">([</span><span class="sa">r</span><span class="s1">&#39;true position&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;UKF&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;GPS measurements&#39;</span><span class="p">])</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">axis</span><span class="p">(</span><span class="s1">&#39;equal&#39;</span><span class="p">)</span>

        <span class="n">ukf3sigma</span> <span class="o">=</span> <span class="mi">3</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">ukf_Ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>
        <span class="n">fig</span><span class="p">,</span> <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">subplots</span><span class="p">(</span><span class="n">figsize</span><span class="o">=</span><span class="p">(</span><span class="mi">10</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="n">xlabel</span><span class="o">=</span><span class="s1">&#39;$t$ (s)&#39;</span><span class="p">,</span> <span class="n">ylabel</span><span class="o">=</span><span class="s1">&#39;error (deg)&#39;</span><span class="p">,</span>
               <span class="n">title</span><span class="o">=</span><span class="s2">&quot;Robot orientation error (deg)&quot;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">errors</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">ukf3sigma</span><span class="p">,</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">,</span> <span class="n">linestyle</span><span class="o">=</span><span class="s1">&#39;dashed&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="p">(</span><span class="o">-</span><span class="n">ukf3sigma</span><span class="p">),</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">,</span> <span class="n">linestyle</span><span class="o">=</span><span class="s1">&#39;dashed&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">legend</span><span class="p">([</span><span class="sa">r</span><span class="s1">&#39;UKF&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;$3\sigma$ UKF&#39;</span><span class="p">])</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set_xlim</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="n">t</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">])</span>

        <span class="n">ukf3sigma</span> <span class="o">=</span> <span class="mi">3</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">ukf_Ps</span><span class="p">[:,</span> <span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">+</span> <span class="n">ukf_Ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>
        <span class="n">fig</span><span class="p">,</span> <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">subplots</span><span class="p">(</span><span class="n">figsize</span><span class="o">=</span><span class="p">(</span><span class="mi">10</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="n">xlabel</span><span class="o">=</span><span class="s1">&#39;$t$ (s)&#39;</span><span class="p">,</span> <span class="n">ylabel</span><span class="o">=</span><span class="s1">&#39;error (m)&#39;</span><span class="p">,</span>
               <span class="n">title</span><span class="o">=</span><span class="s1">&#39;Robot position error (m)&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="n">errors</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="n">ukf3sigma</span><span class="p">,</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">,</span> <span class="n">linestyle</span><span class="o">=</span><span class="s1">&#39;dashed&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">legend</span><span class="p">([</span><span class="sa">r</span><span class="s1">&#39;UKF&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;$3\sigma$ UKF&#39;</span><span class="p">])</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set_xlim</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="n">t</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">])</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set_ylim</span><span class="p">(</span><span class="n">bottom</span><span class="o">=</span><span class="mi">0</span><span class="p">)</span>

    <span class="k">def</span> <span class="nf">plot_traj</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">states</span><span class="p">,</span> <span class="n">ys</span><span class="p">):</span>
        <span class="n">Rots</span><span class="p">,</span> <span class="n">ps</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">states</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>
        <span class="n">fig</span><span class="p">,</span> <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">subplots</span><span class="p">(</span><span class="n">figsize</span><span class="o">=</span><span class="p">(</span><span class="mi">10</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="n">xlabel</span><span class="o">=</span><span class="s1">&#39;$x$ (m)&#39;</span><span class="p">,</span> <span class="n">ylabel</span><span class="o">=</span><span class="s1">&#39;$y$ (m)&#39;</span><span class="p">,</span> <span class="n">title</span><span class="o">=</span><span class="s2">&quot;Robot position&quot;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">linewidth</span><span class="o">=</span><span class="mi">2</span><span class="p">,</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;black&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">scatter</span><span class="p">(</span><span class="n">ys</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">ys</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;red&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">legend</span><span class="p">([</span><span class="sa">r</span><span class="s1">&#39;true position&#39;</span><span class="p">,</span>
                   <span class="sa">r</span><span class="s1">&#39;GPS measurements&#39;</span><span class="p">])</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">axis</span><span class="p">(</span><span class="s1">&#39;equal&#39;</span><span class="p">)</span>

    <span class="k">def</span> <span class="nf">benchmark_plot</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">ukf_err</span><span class="p">,</span> <span class="n">left_ukf_err</span><span class="p">,</span> <span class="n">right_ukf_err</span><span class="p">,</span> <span class="n">iekf_err</span><span class="p">,</span>
                       <span class="n">ekf_err</span><span class="p">,</span> <span class="n">ps</span><span class="p">,</span> <span class="n">ukf_ps</span><span class="p">,</span> <span class="n">left_ukf_ps</span><span class="p">,</span> <span class="n">right_ukf_ps</span><span class="p">,</span> <span class="n">ekf_ps</span><span class="p">,</span>
                       <span class="n">iekf_ps</span><span class="p">):</span>
        <span class="k">def</span> <span class="nf">rmse</span><span class="p">(</span><span class="n">errs</span><span class="p">):</span>
            <span class="n">err</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="n">errs</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">1</span><span class="p">],</span> <span class="mi">2</span><span class="p">))</span>
            <span class="n">err</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">mean</span><span class="p">(</span><span class="n">errs</span><span class="p">[:,</span> <span class="p">:,</span> <span class="mi">0</span><span class="p">]</span><span class="o">**</span><span class="mi">2</span><span class="p">,</span> <span class="n">axis</span><span class="o">=</span><span class="mi">0</span><span class="p">))</span>
            <span class="n">err</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">mean</span><span class="p">(</span><span class="n">errs</span><span class="p">[:,</span> <span class="p">:,</span> <span class="mi">1</span><span class="p">]</span><span class="o">**</span><span class="mi">2</span>
                                        <span class="o">+</span> <span class="n">errs</span><span class="p">[:,</span> <span class="p">:,</span> <span class="mi">2</span><span class="p">]</span><span class="o">**</span><span class="mi">2</span><span class="p">,</span> <span class="n">axis</span><span class="o">=</span><span class="mi">0</span><span class="p">))</span>
            <span class="k">return</span> <span class="n">err</span>

        <span class="n">ukf_e</span> <span class="o">=</span> <span class="n">rmse</span><span class="p">(</span><span class="n">ukf_err</span><span class="p">)</span>
        <span class="n">left_ukf_e</span> <span class="o">=</span> <span class="n">rmse</span><span class="p">(</span><span class="n">left_ukf_err</span><span class="p">)</span>
        <span class="n">right_ukf_e</span> <span class="o">=</span> <span class="n">rmse</span><span class="p">(</span><span class="n">right_ukf_err</span><span class="p">)</span>
        <span class="n">iekf_e</span> <span class="o">=</span> <span class="n">rmse</span><span class="p">(</span><span class="n">iekf_err</span><span class="p">)</span>
        <span class="n">ekf_e</span> <span class="o">=</span> <span class="n">rmse</span><span class="p">(</span><span class="n">ekf_err</span><span class="p">)</span>

        <span class="c1"># get orientation error</span>
        <span class="n">t</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">linspace</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span>  <span class="bp">self</span><span class="o">.</span><span class="n">dt</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">,</span>  <span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>

        <span class="c1"># plot position</span>
        <span class="n">fig</span><span class="p">,</span> <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">subplots</span><span class="p">(</span><span class="n">figsize</span><span class="o">=</span><span class="p">(</span><span class="mi">10</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="n">xlabel</span><span class="o">=</span><span class="s1">&#39;$y$ (m)&#39;</span><span class="p">,</span> <span class="n">ylabel</span><span class="o">=</span><span class="s1">&#39;$x$ (m)&#39;</span><span class="p">,</span>
               <span class="n">title</span><span class="o">=</span><span class="s1">&#39;Robot position for a Monte-Carlo run&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">linewidth</span><span class="o">=</span><span class="mi">2</span><span class="p">,</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;black&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">ukf_ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">ukf_ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;magenta&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">left_ukf_ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">left_ukf_ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;green&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">right_ukf_ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">right_ukf_ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;cyan&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">ekf_ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">ekf_ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;red&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">iekf_ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">iekf_ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">axis</span><span class="p">(</span><span class="s1">&#39;equal&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">legend</span><span class="p">([</span><span class="sa">r</span><span class="s1">&#39;true position&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;$SO(2) \times \mathbb</span><span class="si">{R}</span><span class="s1">^2$ UKF&#39;</span><span class="p">,</span>
                   <span class="sa">r</span><span class="s1">&#39;\textbf{$SE(2)$ UKF (left)}&#39;</span><span class="p">,</span>
                   <span class="sa">r</span><span class="s1">&#39;\textbf{$SE(2)$ UKF (right)}&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;EKF&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;IEKF [BB17]&#39;</span><span class="p">])</span>

        <span class="c1"># plot attitude error</span>
        <span class="n">fig</span><span class="p">,</span> <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">subplots</span><span class="p">(</span><span class="n">figsize</span><span class="o">=</span><span class="p">(</span><span class="mi">10</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="n">xlabel</span><span class="o">=</span><span class="s1">&#39;$t$ (s)&#39;</span><span class="p">,</span> <span class="n">ylabel</span><span class="o">=</span><span class="s1">&#39;error (deg)&#39;</span><span class="p">,</span>
               <span class="n">title</span><span class="o">=</span><span class="s1">&#39;Robot orientation error (deg)&#39;</span><span class="p">)</span>

        <span class="c1"># error</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">ukf_e</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;magenta&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">left_ukf_e</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;green&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">right_ukf_e</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;cyan&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">ekf_e</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;red&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">iekf_e</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">legend</span><span class="p">([</span><span class="sa">r</span><span class="s1">&#39;$SO(2) \times \mathbb</span><span class="si">{R}</span><span class="s1">^2$ UKF&#39;</span><span class="p">,</span>
                   <span class="sa">r</span><span class="s1">&#39;\textbf{$SE(2)$ UKF (left)}&#39;</span><span class="p">,</span>
                   <span class="sa">r</span><span class="s1">&#39;\textbf{$SE(2)$ UKF (right)}&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;EKF&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;IEKF [BB17]&#39;</span><span class="p">])</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set_ylim</span><span class="p">(</span><span class="n">bottom</span><span class="o">=</span><span class="mi">0</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set_xlim</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="n">t</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">])</span>

        <span class="c1"># plot position error</span>
        <span class="n">fig</span><span class="p">,</span> <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">subplots</span><span class="p">(</span><span class="n">figsize</span><span class="o">=</span><span class="p">(</span><span class="mi">10</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="n">xlabel</span><span class="o">=</span><span class="s1">&#39;$t$ (s)&#39;</span><span class="p">,</span> <span class="n">ylabel</span><span class="o">=</span><span class="s1">&#39;error (m)&#39;</span><span class="p">,</span>
               <span class="n">title</span><span class="o">=</span><span class="s1">&#39;Robot position error (m)&#39;</span><span class="p">)</span>

        <span class="c1"># error</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="n">ukf_e</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;magenta&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="n">left_ukf_e</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;green&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="n">right_ukf_e</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;cyan&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="n">ekf_e</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;red&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="n">iekf_e</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">legend</span><span class="p">([</span><span class="sa">r</span><span class="s1">&#39;$SO(2) \times \mathbb</span><span class="si">{R}</span><span class="s1">^2$ UKF&#39;</span><span class="p">,</span>
                   <span class="sa">r</span><span class="s1">&#39;\textbf{$SE(2)$ UKF (left)}&#39;</span><span class="p">,</span> 
                   <span class="sa">r</span><span class="s1">&#39;\textbf{$SE(2)$ UKF (right)}&#39;</span><span class="p">,</span>
                   <span class="sa">r</span><span class="s1">&#39;EKF&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;IEKF [BB17]&#39;</span><span class="p">])</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set_ylim</span><span class="p">(</span><span class="n">bottom</span><span class="o">=</span><span class="mi">0</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set_xlim</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="n">t</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">])</span>
        <span class="k">return</span> <span class="n">ukf_e</span><span class="p">,</span> <span class="n">left_ukf_e</span><span class="p">,</span> <span class="n">right_ukf_e</span><span class="p">,</span> <span class="n">iekf_e</span><span class="p">,</span> <span class="n">ekf_e</span>

    <span class="k">def</span> <span class="nf">benchmark_print</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">ukf_e</span><span class="p">,</span> <span class="n">left_ukf_e</span><span class="p">,</span> <span class="n">right_ukf_e</span><span class="p">,</span> <span class="n">iekf_e</span><span class="p">,</span> <span class="n">ekf_e</span><span class="p">):</span>
        <span class="k">def</span> <span class="nf">rmse</span><span class="p">(</span><span class="n">errs</span><span class="p">):</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">mean</span><span class="p">(</span><span class="n">errs</span><span class="o">**</span><span class="mi">2</span><span class="p">))</span>
        <span class="n">ukf_err_p</span> <span class="o">=</span> <span class="s1">&#39;</span><span class="si">{:.2f}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">rmse</span><span class="p">(</span><span class="n">ukf_e</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]))</span>
        <span class="n">left_ukf_err_p</span> <span class="o">=</span> <span class="s1">&#39;</span><span class="si">{:.2f}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">rmse</span><span class="p">(</span><span class="n">left_ukf_e</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]))</span>
        <span class="n">right_ukf_err_p</span> <span class="o">=</span> <span class="s1">&#39;</span><span class="si">{:.2f}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">rmse</span><span class="p">(</span><span class="n">right_ukf_e</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]))</span>
        <span class="n">ekf_err_p</span> <span class="o">=</span> <span class="s1">&#39;</span><span class="si">{:.2f}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">rmse</span><span class="p">(</span><span class="n">ekf_e</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]))</span>
        <span class="n">iekf_err_p</span> <span class="o">=</span> <span class="s1">&#39;</span><span class="si">{:.2f}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="n">rmse</span><span class="p">(</span><span class="n">iekf_e</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]))</span>

        <span class="n">ukf_err_rot</span> <span class="o">=</span> <span class="s1">&#39;</span><span class="si">{:.2f}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">rmse</span><span class="p">(</span><span class="n">ukf_e</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]))</span>
        <span class="n">left_ukf_err_rot</span> <span class="o">=</span> <span class="s1">&#39;</span><span class="si">{:.2f}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">rmse</span><span class="p">(</span><span class="n">left_ukf_e</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]))</span>
        <span class="n">right_ukf_err_rot</span> <span class="o">=</span> <span class="s1">&#39;</span><span class="si">{:.2f}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">rmse</span><span class="p">(</span><span class="n">right_ukf_e</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]))</span>
        <span class="n">ekf_err_rot</span> <span class="o">=</span> <span class="s1">&#39;</span><span class="si">{:.2f}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">rmse</span><span class="p">(</span><span class="n">ekf_e</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]))</span>
        <span class="n">iekf_err_rot</span> <span class="o">=</span> <span class="s1">&#39;</span><span class="si">{:.2f}</span><span class="s1">&#39;</span><span class="o">.</span><span class="n">format</span><span class="p">(</span><span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">rmse</span><span class="p">(</span><span class="n">iekf_e</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]))</span>

        <span class="nb">print</span><span class="p">(</span><span class="s1">&#39; &#39;</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s1">&#39;Root Mean Square Error w.r.t. orientation (deg)&#39;</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -SO(2) x R^2 UKF: &quot;</span> <span class="o">+</span> <span class="n">ukf_err_rot</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -left SE(2) UKF : &quot;</span> <span class="o">+</span> <span class="n">left_ukf_err_rot</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -right SE(2) UKF: &quot;</span> <span class="o">+</span> <span class="n">right_ukf_err_rot</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -EKF            : &quot;</span> <span class="o">+</span> <span class="n">ekf_err_rot</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -IEKF           : &quot;</span> <span class="o">+</span> <span class="n">iekf_err_rot</span><span class="p">)</span>

        <span class="nb">print</span><span class="p">(</span><span class="s1">&#39; &#39;</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s1">&#39;Root Mean Square Error w.r.t. position (m)&#39;</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -SO(2) x R^2 UKF: &quot;</span> <span class="o">+</span> <span class="n">ukf_err_p</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -left SE(2) UKF : &quot;</span> <span class="o">+</span> <span class="n">left_ukf_err_p</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -right SE(2) UKF: &quot;</span> <span class="o">+</span> <span class="n">right_ukf_err_p</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -EKF            : &quot;</span> <span class="o">+</span> <span class="n">ekf_err_p</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -IEKF           : &quot;</span> <span class="o">+</span> <span class="n">iekf_err_p</span><span class="p">)</span>

    <span class="k">def</span> <span class="nf">nees</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">err</span><span class="p">,</span> <span class="n">Ps</span><span class="p">,</span> <span class="n">Rots</span><span class="p">,</span> <span class="n">ps</span><span class="p">,</span> <span class="n">name</span><span class="p">):</span>

        <span class="n">neess</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">,</span> <span class="mi">2</span><span class="p">))</span>
        <span class="n">J</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">eye</span><span class="p">(</span><span class="mi">3</span><span class="p">)</span>

        <span class="k">def</span> <span class="nf">err2nees</span><span class="p">(</span><span class="n">err</span><span class="p">,</span> <span class="n">P</span><span class="p">):</span>
            <span class="c1"># separate orientation and position</span>
            <span class="n">nees_Rot</span> <span class="o">=</span> <span class="n">err</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span><span class="o">**</span><span class="mi">2</span> <span class="o">/</span> <span class="n">P</span><span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span>
            <span class="n">nees_p</span> <span class="o">=</span> <span class="n">err</span><span class="p">[</span><span class="mi">1</span><span class="p">:</span><span class="mi">3</span><span class="p">]</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">inv</span><span class="p">(</span><span class="n">P</span><span class="p">[</span><span class="mi">1</span><span class="p">:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">1</span><span class="p">:</span><span class="mi">3</span><span class="p">])</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">err</span><span class="p">[</span><span class="mi">1</span><span class="p">:</span><span class="mi">3</span><span class="p">]))</span><span class="o">/</span><span class="mi">2</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="n">nees_Rot</span><span class="p">,</span> <span class="n">nees_p</span><span class="p">])</span>

        <span class="c1"># only after 20 s</span>
        <span class="k">for</span> <span class="n">n</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="mi">2000</span><span class="p">,</span> <span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">):</span>
            <span class="c1"># covariance need to be turned</span>
            <span class="k">if</span> <span class="n">name</span> <span class="o">==</span> <span class="s1">&#39;STD&#39;</span><span class="p">:</span>
                <span class="n">P</span> <span class="o">=</span> <span class="n">Ps</span><span class="p">[</span><span class="n">n</span><span class="p">]</span>
            <span class="k">elif</span> <span class="n">name</span> <span class="o">==</span> <span class="s1">&#39;LEFT&#39;</span><span class="p">:</span>
                <span class="n">J</span><span class="p">[</span><span class="mi">1</span><span class="p">:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">1</span><span class="p">:</span><span class="mi">3</span><span class="p">]</span> <span class="o">=</span> <span class="n">Rots</span><span class="p">[</span><span class="n">n</span><span class="p">]</span>
                <span class="n">P</span> <span class="o">=</span> <span class="n">J</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">Ps</span><span class="p">[</span><span class="n">n</span><span class="p">])</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">J</span><span class="o">.</span><span class="n">T</span><span class="p">)</span>
            <span class="k">else</span><span class="p">:</span>
                <span class="n">J</span><span class="p">[</span><span class="mi">1</span><span class="p">:</span><span class="mi">3</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="bp">self</span><span class="o">.</span><span class="n">J</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">ps</span><span class="p">[</span><span class="n">n</span><span class="p">])</span>
                <span class="n">P</span> <span class="o">=</span> <span class="n">J</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">Ps</span><span class="p">[</span><span class="n">n</span><span class="p">])</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">J</span><span class="o">.</span><span class="n">T</span><span class="p">)</span>
            <span class="n">neess</span><span class="p">[</span><span class="n">n</span><span class="p">]</span> <span class="o">=</span> <span class="n">err2nees</span><span class="p">(</span><span class="n">err</span><span class="p">[</span><span class="n">n</span><span class="p">],</span> <span class="n">P</span><span class="p">)</span>
        <span class="k">return</span> <span class="n">neess</span>

    <span class="k">def</span> <span class="nf">nees_print</span><span class="p">(</span><span class="bp">self</span><span class="p">,</span> <span class="n">ukf_nees</span><span class="p">,</span> <span class="n">left_ukf_nees</span><span class="p">,</span> <span class="n">right_ukf_nees</span><span class="p">,</span> <span class="n">iekf_nees</span><span class="p">,</span>
                   <span class="n">ekf_nees</span><span class="p">):</span>

        <span class="n">t</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">linspace</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span>  <span class="bp">self</span><span class="o">.</span><span class="n">dt</span> <span class="o">*</span> <span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">,</span>  <span class="bp">self</span><span class="o">.</span><span class="n">N</span><span class="p">)</span>

        <span class="k">def</span> <span class="nf">f</span><span class="p">(</span><span class="n">x</span><span class="p">):</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">mean</span><span class="p">(</span><span class="n">x</span><span class="p">,</span> <span class="n">axis</span><span class="o">=</span><span class="mi">0</span><span class="p">)</span>
        <span class="n">ukf_nees</span> <span class="o">=</span> <span class="n">f</span><span class="p">(</span><span class="n">ukf_nees</span><span class="p">)</span>
        <span class="n">left_ukf_nees</span> <span class="o">=</span> <span class="n">f</span><span class="p">(</span><span class="n">left_ukf_nees</span><span class="p">)</span>
        <span class="n">right_ukf_nees</span> <span class="o">=</span> <span class="n">f</span><span class="p">(</span><span class="n">right_ukf_nees</span><span class="p">)</span>
        <span class="n">iekf_nees</span> <span class="o">=</span> <span class="n">f</span><span class="p">(</span><span class="n">iekf_nees</span><span class="p">)</span>
        <span class="n">ekf_nees</span> <span class="o">=</span> <span class="n">f</span><span class="p">(</span><span class="n">ekf_nees</span><span class="p">)</span>

        <span class="n">m</span> <span class="o">=</span> <span class="mi">2000</span>

        <span class="c1"># plot orientation nees</span>
        <span class="n">fig</span><span class="p">,</span> <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">subplots</span><span class="p">(</span><span class="n">figsize</span><span class="o">=</span><span class="p">(</span><span class="mi">10</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="n">xlabel</span><span class="o">=</span><span class="s1">&#39;$t$ (s)&#39;</span><span class="p">,</span> <span class="n">ylabel</span><span class="o">=</span><span class="s1">&#39;orientation NEES&#39;</span><span class="p">,</span>
               <span class="n">title</span><span class="o">=</span><span class="s1">&#39;Robot orientation NEES&#39;</span><span class="p">,</span> <span class="n">yscale</span><span class="o">=</span><span class="s2">&quot;log&quot;</span><span class="p">)</span>

        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">[</span><span class="n">m</span><span class="p">:],</span> <span class="n">ukf_nees</span><span class="p">[</span><span class="n">m</span><span class="p">:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;magenta&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">[</span><span class="n">m</span><span class="p">:],</span> <span class="n">left_ukf_nees</span><span class="p">[</span><span class="n">m</span><span class="p">:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;green&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">[</span><span class="n">m</span><span class="p">:],</span> <span class="n">right_ukf_nees</span><span class="p">[</span><span class="n">m</span><span class="p">:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;cyan&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">[</span><span class="n">m</span><span class="p">:],</span> <span class="n">ekf_nees</span><span class="p">[</span><span class="n">m</span><span class="p">:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;red&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">[</span><span class="n">m</span><span class="p">:],</span> <span class="n">iekf_nees</span><span class="p">[</span><span class="n">m</span><span class="p">:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">legend</span><span class="p">([</span><span class="sa">r</span><span class="s1">&#39;$SO(2) \times \mathbb</span><span class="si">{R}</span><span class="s1">^2$ UKF&#39;</span><span class="p">,</span>
                   <span class="sa">r</span><span class="s1">&#39;\textbf{$SE(2)$ UKF (left)}&#39;</span><span class="p">,</span> 
                   <span class="sa">r</span><span class="s1">&#39;\textbf{$SE(2)$ UKF (right)}&#39;</span><span class="p">,</span>
                   <span class="sa">r</span><span class="s1">&#39;EKF&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;IEKF [BB17]&#39;</span><span class="p">])</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set_xlim</span><span class="p">(</span><span class="n">t</span><span class="p">[</span><span class="n">m</span><span class="p">],</span> <span class="n">t</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">])</span>

        <span class="c1"># plot position nees</span>
        <span class="n">fig</span><span class="p">,</span> <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">subplots</span><span class="p">(</span><span class="n">figsize</span><span class="o">=</span><span class="p">(</span><span class="mi">10</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="n">xlabel</span><span class="o">=</span><span class="s1">&#39;$t$ (s)&#39;</span><span class="p">,</span> <span class="n">ylabel</span><span class="o">=</span><span class="s1">&#39;position NEES&#39;</span><span class="p">,</span>
               <span class="n">title</span><span class="o">=</span><span class="s1">&#39;Robot position NEES&#39;</span><span class="p">,</span> <span class="n">yscale</span><span class="o">=</span><span class="s2">&quot;log&quot;</span><span class="p">)</span>

        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">[</span><span class="n">m</span><span class="p">:],</span> <span class="n">ukf_nees</span><span class="p">[</span><span class="n">m</span><span class="p">:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;magenta&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">[</span><span class="n">m</span><span class="p">:],</span> <span class="n">left_ukf_nees</span><span class="p">[</span><span class="n">m</span><span class="p">:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;green&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">[</span><span class="n">m</span><span class="p">:],</span> <span class="n">right_ukf_nees</span><span class="p">[</span><span class="n">m</span><span class="p">:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;cyan&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">[</span><span class="n">m</span><span class="p">:],</span> <span class="n">ekf_nees</span><span class="p">[</span><span class="n">m</span><span class="p">:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;red&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">[</span><span class="n">m</span><span class="p">:],</span> <span class="n">iekf_nees</span><span class="p">[</span><span class="n">m</span><span class="p">:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">legend</span><span class="p">([</span><span class="sa">r</span><span class="s1">&#39;$SO(2) \times \mathbb</span><span class="si">{R}</span><span class="s1">^2$ UKF&#39;</span><span class="p">,</span>
                   <span class="sa">r</span><span class="s1">&#39;\textbf{$SE(2)$ UKF (left)}&#39;</span><span class="p">,</span> 
                   <span class="sa">r</span><span class="s1">&#39;\textbf{$SE(2)$ UKF (right)}&#39;</span><span class="p">,</span>
                   <span class="sa">r</span><span class="s1">&#39;EKF&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;IEKF [BB17]&#39;</span><span class="p">])</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set_xlim</span><span class="p">(</span><span class="n">t</span><span class="p">[</span><span class="n">m</span><span class="p">],</span> <span class="n">t</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">])</span>

        <span class="k">def</span> <span class="nf">g</span><span class="p">(</span><span class="n">x</span><span class="p">):</span>
            <span class="c1"># only after 20 s</span>
            <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">mean</span><span class="p">(</span><span class="n">x</span><span class="p">[</span><span class="n">m</span><span class="p">:])</span>

        <span class="nb">print</span><span class="p">(</span><span class="s1">&#39; &#39;</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s1">&#39; Normalized Estimation Error Squared (NEES) w.r.t. orientation&#39;</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -SO(2) x R^2 UKF: </span><span class="si">% .2f</span><span class="s2"> &quot;</span> <span class="o">%</span> <span class="n">g</span><span class="p">(</span><span class="n">ukf_nees</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]))</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -left SE(2) UKF : </span><span class="si">% .2f</span><span class="s2"> &quot;</span> <span class="o">%</span> <span class="n">g</span><span class="p">(</span><span class="n">left_ukf_nees</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]))</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -right SE(2) UKF: </span><span class="si">% .2f</span><span class="s2"> &quot;</span> <span class="o">%</span> <span class="n">g</span><span class="p">(</span><span class="n">right_ukf_nees</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]))</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -EKF            : </span><span class="si">% .2f</span><span class="s2"> &quot;</span> <span class="o">%</span> <span class="n">g</span><span class="p">(</span><span class="n">ekf_nees</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]))</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -IEKF           : </span><span class="si">% .2f</span><span class="s2"> &quot;</span> <span class="o">%</span> <span class="n">g</span><span class="p">(</span><span class="n">iekf_nees</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]))</span>

        <span class="nb">print</span><span class="p">(</span><span class="s1">&#39; &#39;</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s1">&#39; Normalized Estimation Error Squared (NEES) w.r.t. position&#39;</span><span class="p">)</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -SO(2) x R^2 UKF: </span><span class="si">% .2f</span><span class="s2"> &quot;</span> <span class="o">%</span> <span class="n">g</span><span class="p">(</span><span class="n">ukf_nees</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]))</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -left SE(2) UKF : </span><span class="si">% .2f</span><span class="s2"> &quot;</span> <span class="o">%</span> <span class="n">g</span><span class="p">(</span><span class="n">left_ukf_nees</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]))</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -right SE(2) UKF: </span><span class="si">% .2f</span><span class="s2"> &quot;</span> <span class="o">%</span> <span class="n">g</span><span class="p">(</span><span class="n">right_ukf_nees</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]))</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -EKF            : </span><span class="si">% .2f</span><span class="s2"> &quot;</span> <span class="o">%</span> <span class="n">g</span><span class="p">(</span><span class="n">ekf_nees</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]))</span>
        <span class="nb">print</span><span class="p">(</span><span class="s2">&quot;    -IEKF           : </span><span class="si">% .2f</span><span class="s2"> &quot;</span> <span class="o">%</span> <span class="n">g</span><span class="p">(</span><span class="n">iekf_nees</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]))</span>

    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">load</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">n_sequence</span><span class="p">,</span> <span class="n">gps_freq</span><span class="p">,</span> <span class="n">gps_std</span><span class="p">):</span>

        <span class="n">f_name</span> <span class="o">=</span> <span class="n">os</span><span class="o">.</span><span class="n">path</span><span class="o">.</span><span class="n">join</span><span class="p">(</span><span class="bp">cls</span><span class="o">.</span><span class="n">data_dir</span><span class="p">,</span> <span class="s2">&quot;wifibot&quot;</span>
                              <span class="o">+</span> <span class="nb">str</span><span class="p">(</span><span class="n">n_sequence</span><span class="p">)</span> <span class="o">+</span> <span class="s2">&quot;.txt&quot;</span><span class="p">)</span>
        <span class="n">data</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">genfromtxt</span><span class="p">(</span><span class="n">f_name</span><span class="p">,</span> <span class="n">skip_header</span><span class="o">=</span><span class="mi">1</span><span class="p">)</span>
        <span class="n">t</span> <span class="o">=</span> <span class="n">data</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">]</span>
        <span class="n">gyros</span> <span class="o">=</span> <span class="n">data</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span>
        <span class="n">vs</span> <span class="o">=</span> <span class="n">data</span><span class="p">[:,</span> <span class="mi">2</span><span class="p">:</span><span class="mi">4</span><span class="p">]</span>
        <span class="n">thetas</span> <span class="o">=</span> <span class="n">data</span><span class="p">[:,</span> <span class="mi">4</span><span class="p">]</span>
        <span class="n">ps</span> <span class="o">=</span> <span class="n">data</span><span class="p">[:,</span> <span class="mi">5</span><span class="p">:</span><span class="mi">7</span><span class="p">]</span>
        <span class="n">states</span> <span class="o">=</span> <span class="p">[]</span>
        <span class="n">omegas</span> <span class="o">=</span> <span class="p">[]</span>

        <span class="k">for</span> <span class="n">n</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">t</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]):</span>
            <span class="n">states</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="bp">cls</span><span class="o">.</span><span class="n">STATE</span><span class="p">(</span><span class="n">Rot</span><span class="o">=</span><span class="n">SO2</span><span class="o">.</span><span class="n">exp</span><span class="p">(</span><span class="n">thetas</span><span class="p">[</span><span class="n">n</span><span class="p">]),</span>
                                    <span class="n">p</span><span class="o">=</span><span class="n">ps</span><span class="p">[</span><span class="n">n</span><span class="p">]))</span>
            <span class="n">omegas</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="bp">cls</span><span class="o">.</span><span class="n">INPUT</span><span class="p">(</span><span class="n">gyro</span><span class="o">=</span><span class="n">gyros</span><span class="p">[</span><span class="n">n</span><span class="p">],</span>
                                    <span class="n">v</span><span class="o">=</span><span class="n">vs</span><span class="p">[</span><span class="n">n</span><span class="p">]))</span>

        <span class="c1"># simulate measurements</span>
        <span class="n">N</span> <span class="o">=</span> <span class="n">t</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>
        <span class="n">one_hot_ys</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">(</span><span class="n">N</span><span class="p">)</span>
        <span class="n">t_gps</span> <span class="o">=</span> <span class="n">t</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">+</span> <span class="n">np</span><span class="o">.</span><span class="n">arange</span><span class="p">(</span><span class="mi">2000</span><span class="p">)</span><span class="o">/</span><span class="n">gps_freq</span>
        <span class="n">k</span> <span class="o">=</span> <span class="mi">1</span>
        <span class="n">ys</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="n">t_gps</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">],</span> <span class="mi">2</span><span class="p">))</span>
        <span class="k">for</span> <span class="n">n</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">N</span><span class="p">):</span>
            <span class="k">if</span> <span class="n">t_gps</span><span class="p">[</span><span class="n">k</span><span class="p">]</span> <span class="o">&lt;=</span> <span class="n">t</span><span class="p">[</span><span class="n">n</span><span class="p">]:</span>
                <span class="n">ys</span><span class="p">[</span><span class="n">k</span><span class="p">]</span> <span class="o">=</span> <span class="n">states</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">.</span><span class="n">p</span> <span class="o">+</span> <span class="n">gps_std</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">random</span><span class="o">.</span><span class="n">randn</span><span class="p">(</span><span class="mi">2</span><span class="p">)</span>
                <span class="n">one_hot_ys</span><span class="p">[</span><span class="n">n</span><span class="p">]</span> <span class="o">=</span> <span class="mi">1</span>
                <span class="n">k</span> <span class="o">=</span> <span class="n">k</span> <span class="o">+</span> <span class="mi">1</span>

        <span class="n">ys</span> <span class="o">=</span> <span class="n">ys</span><span class="p">[:</span><span class="n">k</span><span class="p">]</span>
        <span class="k">return</span> <span class="n">states</span><span class="p">,</span> <span class="n">omegas</span><span class="p">,</span> <span class="n">ys</span><span class="p">,</span> <span class="n">one_hot_ys</span><span class="p">,</span> <span class="n">t</span>

    <span class="nd">@classmethod</span>
    <span class="k">def</span> <span class="nf">plot_wifibot</span><span class="p">(</span><span class="bp">cls</span><span class="p">,</span> <span class="n">ukf_states</span><span class="p">,</span> <span class="n">ukf_Ps</span><span class="p">,</span> <span class="n">states</span><span class="p">,</span> <span class="n">ys</span><span class="p">,</span> <span class="n">t</span><span class="p">):</span>

        <span class="n">N</span> <span class="o">=</span> <span class="n">t</span><span class="o">.</span><span class="n">shape</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span>

        <span class="n">Rots</span><span class="p">,</span> <span class="n">ps</span> <span class="o">=</span> <span class="bp">cls</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">states</span><span class="p">,</span> <span class="n">N</span><span class="p">)</span>
        <span class="n">ukf_Rots</span><span class="p">,</span> <span class="n">ukf_ps</span> <span class="o">=</span> <span class="bp">cls</span><span class="o">.</span><span class="n">get_states</span><span class="p">(</span><span class="n">ukf_states</span><span class="p">,</span> <span class="n">N</span><span class="p">)</span>
        <span class="n">errors</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">zeros</span><span class="p">((</span><span class="n">N</span><span class="p">,</span> <span class="mi">2</span><span class="p">))</span>
        <span class="k">for</span> <span class="n">n</span> <span class="ow">in</span> <span class="nb">range</span><span class="p">(</span><span class="n">N</span><span class="p">):</span>
            <span class="n">errors</span><span class="p">[</span><span class="n">n</span><span class="p">,</span> <span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="n">SO2</span><span class="o">.</span><span class="n">log</span><span class="p">(</span><span class="n">Rots</span><span class="p">[</span><span class="n">n</span><span class="p">]</span><span class="o">.</span><span class="n">T</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">ukf_Rots</span><span class="p">[</span><span class="n">n</span><span class="p">]))</span>
        <span class="n">errors</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">linalg</span><span class="o">.</span><span class="n">norm</span><span class="p">(</span><span class="n">ps</span> <span class="o">-</span> <span class="n">ukf_ps</span><span class="p">,</span> <span class="n">axis</span><span class="o">=</span><span class="mi">1</span><span class="p">)</span>

        <span class="n">fig</span><span class="p">,</span> <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">subplots</span><span class="p">(</span><span class="n">figsize</span><span class="o">=</span><span class="p">(</span><span class="mi">10</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="n">xlabel</span><span class="o">=</span><span class="s1">&#39;$x$ (m)&#39;</span><span class="p">,</span> <span class="n">ylabel</span><span class="o">=</span><span class="s1">&#39;$y$ (m)&#39;</span><span class="p">,</span> <span class="n">title</span><span class="o">=</span><span class="s2">&quot;Robot position&quot;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">linewidth</span><span class="o">=</span><span class="mi">2</span><span class="p">,</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;black&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">scatter</span><span class="p">(</span><span class="n">ys</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">ys</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;red&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">ukf_ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">ukf_ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">legend</span><span class="p">([</span><span class="sa">r</span><span class="s1">&#39;true position&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;UKF&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;GPS measurements&#39;</span><span class="p">])</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">axis</span><span class="p">(</span><span class="s1">&#39;equal&#39;</span><span class="p">)</span>

        <span class="n">ukf3sigma</span> <span class="o">=</span> <span class="mi">3</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">ukf_Ps</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">])</span>
        <span class="n">fig</span><span class="p">,</span> <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">subplots</span><span class="p">(</span><span class="n">figsize</span><span class="o">=</span><span class="p">(</span><span class="mi">10</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="n">xlabel</span><span class="o">=</span><span class="s1">&#39;$t$ (s)&#39;</span><span class="p">,</span> <span class="n">ylabel</span><span class="o">=</span><span class="s1">&#39;error (deg)&#39;</span><span class="p">,</span>
               <span class="n">title</span><span class="o">=</span><span class="s2">&quot;Robot orientation error (deg)&quot;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">errors</span><span class="p">[:,</span> <span class="mi">0</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="n">ukf3sigma</span><span class="p">,</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">,</span> <span class="n">linestyle</span><span class="o">=</span><span class="s1">&#39;dashed&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="mi">180</span><span class="o">/</span><span class="n">np</span><span class="o">.</span><span class="n">pi</span><span class="o">*</span><span class="p">(</span><span class="o">-</span> <span class="n">ukf3sigma</span><span class="p">),</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">,</span> <span class="n">linestyle</span><span class="o">=</span><span class="s1">&#39;dashed&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">legend</span><span class="p">([</span><span class="sa">r</span><span class="s1">&#39;UKF&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;$3\sigma$ UKF&#39;</span><span class="p">])</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set_xlim</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="n">t</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">])</span>

        <span class="n">ukf3sigma</span> <span class="o">=</span> <span class="mi">3</span><span class="o">*</span><span class="n">np</span><span class="o">.</span><span class="n">sqrt</span><span class="p">(</span><span class="n">ukf_Ps</span><span class="p">[:,</span> <span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">]</span> <span class="o">+</span> <span class="n">ukf_Ps</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">,</span> <span class="mi">1</span><span class="p">])</span>
        <span class="n">fig</span><span class="p">,</span> <span class="n">ax</span> <span class="o">=</span> <span class="n">plt</span><span class="o">.</span><span class="n">subplots</span><span class="p">(</span><span class="n">figsize</span><span class="o">=</span><span class="p">(</span><span class="mi">10</span><span class="p">,</span> <span class="mi">6</span><span class="p">))</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set</span><span class="p">(</span><span class="n">xlabel</span><span class="o">=</span><span class="s1">&#39;$t$ (s)&#39;</span><span class="p">,</span> <span class="n">ylabel</span><span class="o">=</span><span class="s1">&#39;error (m)&#39;</span><span class="p">,</span>
               <span class="n">title</span><span class="o">=</span><span class="s1">&#39;Robot position error (m)&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="n">errors</span><span class="p">[:,</span> <span class="mi">1</span><span class="p">],</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="n">ukf3sigma</span><span class="p">,</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">,</span> <span class="n">linestyle</span><span class="o">=</span><span class="s1">&#39;dashed&#39;</span><span class="p">)</span>
        <span class="n">plt</span><span class="o">.</span><span class="n">plot</span><span class="p">(</span><span class="n">t</span><span class="p">,</span> <span class="o">-</span><span class="n">ukf3sigma</span><span class="p">,</span> <span class="n">c</span><span class="o">=</span><span class="s1">&#39;blue&#39;</span><span class="p">,</span> <span class="n">linestyle</span><span class="o">=</span><span class="s1">&#39;dashed&#39;</span><span class="p">)</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">legend</span><span class="p">([</span><span class="sa">r</span><span class="s1">&#39;UKF&#39;</span><span class="p">,</span> <span class="sa">r</span><span class="s1">&#39;$3\sigma$ UKF&#39;</span><span class="p">])</span>
        <span class="n">ax</span><span class="o">.</span><span class="n">set_xlim</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="n">t</span><span class="p">[</span><span class="o">-</span><span class="mi">1</span><span class="p">])</span></div>
</pre></div>

           </div>
          </div>
          <footer>
  

  <hr/>

  <div role="contentinfo">
    <p>
        &copy; Copyright 2019, Martin Brossard, Axel Barrau, Silvère Bonnabel.

    </p>
  </div>
  Built with <a href="http://sphinx-doc.org/">Sphinx</a> using a <a href="https://github.com/snide/sphinx_rtd_theme">theme</a> provided by <a href="https://readthedocs.org">Read the Docs</a>. 

</footer>

        </div>
      </div>

    </section>

  </div>
  


  

    <script type="text/javascript">
        var DOCUMENTATION_OPTIONS = {
            URL_ROOT:'../../../',
            VERSION:'alpha',
            COLLAPSE_INDEX:false,
            FILE_SUFFIX:'.html',
            HAS_SOURCE:  true
        };
    </script>
      <script type="text/javascript" src="../../../_static/jquery.js"></script>
      <script type="text/javascript" src="../../../_static/underscore.js"></script>
      <script type="text/javascript" src="../../../_static/doctools.js"></script>
      <script type="text/javascript" src="../../../_static/language_data.js"></script>
      <script type="text/javascript" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/latest.js?config=TeX-AMS-MML_HTMLorMML"></script>

  

  
  
    <script type="text/javascript" src="../../../_static/js/theme.js"></script>
  

  
  
  <script type="text/javascript">
      jQuery(function () {
          SphinxRtdTheme.StickyNav.enable();
      });
  </script>
   

</body>
</html>